from machine import Timer,PWM
import utime
import time
from Maix import GPIO
from fpioa_manager import fm
import sensor, lcd

#PWM通过定时器配置，接到IO17引脚（Pin IO17）
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim1 = Timer(Timer.TIMER0, Timer.CHANNEL1, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=15)
S2 = PWM(tim1, freq=50, duty=0, pin=17)


up_servo_value = 50
down_servo_value = -23

times = 0#在距离较远的两个增加舵机打角的间隔

fm.register(16, fm.fpioa.GPIOHS0)
key = GPIO(GPIO.GPIOHS0, GPIO.IN, GPIO.PULL_UP)


fm.register(13, fm.fpioa.GPIOHS13)
K1 = GPIO(GPIO.GPIOHS13, GPIO.IN,GPIO.PULL_UP)

fm.register(14, fm.fpioa.GPIOHS14)
K2 = GPIO(GPIO.GPIOHS14, GPIO.IN,GPIO.PULL_UP)


fm.register(12, fm.fpioa.GPIO0)
C = GPIO(GPIO.GPIO0, GPIO.OUT)
C.value(0)


sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.run(1)
sensor.skip_frames()

lcd.init(freq=15000000)

print (sensor.width(),sensor.height())

img = sensor.snapshot()

'''
说明：舵机控制函数
功能：180度舵机：angle:-90至90 表示相应的角度
     360连续旋转度舵机：angle:-90至90 旋转方向和速度值。
    【duty】占空比值：0-100
'''
def Servo(servo,angle):
    servo.duty((angle+90)/180*10+2.5)

def turn_pin(pin_num):
    C.value(not C.value())

#def k2_task (pin_num):
#    global up_servo_value,down_servo_value


def k1_task (pin_num):
    global img,up_servo_value,down_servo_value,times
    up_servo_value = 0#为零向上抬高
    down_servo_value = 3
    imgB = img.mean_pooled(6,4)#缩小图片
    imgB = imgB.binary([(80,100)])#白底二值化
    lcd.display(imgB)
    time.sleep_ms(10000)
    for j in range(0,60):#遍历这张二值化图像
        for i in range(0,53):
            num = imgB.get_pixel(i,j)
            if num[0] == 0:
                Servo(S1,up_servo_value+j)    #上
                Servo(S2,down_servo_value-i)   #下
                time.sleep_ms(100+times)
                times = 0
                C.value(1)
            else:
                C.value(0)
                times+=10
        C.value(0)
    up_servo_value = 50
    down_servo_value = -23

key.irq(turn_pin,GPIO.IRQ_FALLING, GPIO.WAKEUP_NOT_SUPPORT,6)
#K2.irq(k2_task,GPIO.IRQ_RISING, GPIO.WAKEUP_NOT_SUPPORT,7)
K1.irq(k1_task,GPIO.IRQ_RISING, GPIO.WAKEUP_NOT_SUPPORT,5)

while True:
    img = sensor.snapshot()
    Servo(S1,up_servo_value)    #上
    Servo(S2,down_servo_value)   #下
    lcd.display(img)





